#include "..\include\BoneSet.h"

float BoneSet::timer = 0;
BoneSet::BoneSet(SpiderBones spiderBones, Vector3 offset, float timeOffset)
:	TIME_OFFSET(timeOffset),
	OFFSET(Vector3(offset.x, -2, offset.z))
{
	_spiderManager = SpiderManager::getSingletonPtr();
	b1 = _spiderManager->getSpiderBonesList()[spiderBones.BONE1];
	b2 = _spiderManager->getSpiderBonesList()[spiderBones.BONE2];
	b3 = _spiderManager->getSpiderBonesList()[spiderBones.BONE3];
	
}

BoneSet::~BoneSet(void)
{
}

Vector3 BoneSet::wp(SceneNode* _node, Bone* _bone)// returns given bone's world position
{
	return _node->_getDerivedPosition() + _node->_getDerivedOrientation() * _node->_getDerivedScale() * _bone->_getDerivedPosition();
}
bool BoneSet::solveIK()
{
	Ogre::Quaternion q = b2->getOrientation();
	Ogre::Matrix3 m;
	q.ToRotationMatrix(m);
	Ogre::Radian x,y,z;
	m.ToEulerAnglesXYZ(x,y,z);

	// which way bone will turn + or -
	int prefSide = y.valueDegrees() < 0 ? -1 : 1;

	// current vector b3 to b1
	Ogre::Vector3 cv = wp(_spiderManager->getSpiderSceneNode(),b3) - wp(_spiderManager->getSpiderSceneNode(),b1);
	cv.normalise();

	// target vector goal to b1
	Ogre::Vector3 tv = goalPosition - wp(_spiderManager->getSpiderSceneNode(),b1);
	tv.normalise();

	Ogre::Vector3 zax = cv.crossProduct(tv);
	zax.normalise();

	Ogre::Real l = cv.dotProduct(tv);
	Ogre::Radian a = Ogre::Math::ACos(l);

	q.FromAngleAxis(a,zax);
	b1->rotate(q,Ogre::Node::TS_WORLD);

	// UPDATE
	b1->_update(true,false);

	float ex = 0;
	float ey = wp(_spiderManager->getSpiderSceneNode(),b1).distance(goalPosition);
	
	float l1 = wp(_spiderManager->getSpiderSceneNode(),b1).distance(wp(_spiderManager->getSpiderSceneNode(),b2));
	float l2 = wp(_spiderManager->getSpiderSceneNode(),b2).distance(wp(_spiderManager->getSpiderSceneNode(),b3));

	float cos2 = ((ex * ex) + (ey * ey) - (l1 * l1) - (l2 * l2)) / (2 * l1 * l2);

	if (cos2 >= -1 && cos2 <= 1)
	{
		Ogre::Radian angle = Ogre::Math::ACos(cos2);
		b2->setOrientation(Ogre::Quaternion(angle*prefSide,-Ogre::Vector3::UNIT_Z));
		// UPDATE
		b1->_update(true,false);

	}

	// current vector b3 to b1
	cv = wp(_spiderManager->getSpiderSceneNode(),b3) - wp(_spiderManager->getSpiderSceneNode(),b1);
	cv.normalise();

	// target vector goal to b1
	tv = goalPosition - wp(_spiderManager->getSpiderSceneNode(),b1);
	tv.normalise();

	zax = cv.crossProduct(tv);
	zax.normalise();

	l = cv.dotProduct(tv);
	a = Ogre::Math::ACos(l);
	
	q.FromAngleAxis(a,zax);
	b1->rotate(q,Ogre::Node::TS_WORLD);

	// UPDATE
	b1->_update(true,false);
	
	return 1;
}
void BoneSet::update(float deltaTime)
{
	/*
	//currentTimer = (timer % (2 * Ogre::Math::PI)) + offsetTime;
	currentTimer = timer / 2;
	currentTimer = 2 * Ogre::Math::PI - currentTimer;
	currentTimer += TIME_OFFSET;
	if(currentTimer < ((2* Ogre::Math::PI) / 4 * 3))
	{	//FLOOR
		goalPosition += Vector3(STEP_LENGTH,0,0) * deltaTime;
	}
	else
	{	//AIR
		airAngle = ((2 * Ogre::Math::PI) - currentTimer )* 2;
		goalPosition -= Vector3(STEP_LENGTH*deltaTime,sin(airAngle),0);
		
	}
	//*/
	goalPosition = Vector3(0, 0.5 * cos(timer + TIME_OFFSET), 1 * sin(timer + TIME_OFFSET));
	goalPosition += Vector3(OFFSET.x,OFFSET.y,OFFSET.z);
	goalPosition += _spiderManager->getSpiderSceneNode()->getPosition();
	solveIK();
}